clc
clear all
%urdf模型导入
robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left.urdf");
% robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left1.urdf");
robot.DataFormat='column';
robot.Gravity =[0 0 -9.81];%%



%%根据urdf模型确定刚体系统自由度
N_fixed = 0;
for i = 1:1:robot.NumBodies
    if( strcmp(robot.Bodies{1, i}.Joint.Type,'fixed') == true)
        N_fixed=N_fixed+1;
    end   
end
N_dof = robot.NumBodies - N_fixed;

%创建机器人刚体系统参数结构体：robot_para
robot_para.JointAxis = zeros(3,N_dof);
robot_para.JointToParentTransform = zeros(4,4,N_dof);
for i = 1:1:N_dof
    robot_para.JointAxis(:,i) = robot.Bodies{1,i+1}.Joint.JointAxis;
    robot_para.JointToParentTransform(:,:,i) = robot.Bodies{1,i+1}.Joint.JointToParentTransform;
end

%定义连杆的惯量 N_dof个自由度的连杆
I_c=zeros(3,3,N_dof);
I_o=zeros(3,3,N_dof);
d_oc=zeros(3,N_dof);
m=zeros(1,N_dof); %质量

%根据所导入的urdf模型读取各连杆的质量 惯量（相对于各刚体坐标系原点） 和质心位置向量（相对于各个刚体坐标系原点）
for i = 1:1:N_dof
    m(i) = robot.Bodies{1, i+1}.Mass; %由于urdf模型中，第一个刚体为固定基，因此第一个活动连杆的索引为2， 也是循环i+1的由来
    d_oc(:,i) = robot.Bodies{1, i+1}.CenterOfMass';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,N_dof);
for i=1:N_dof
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    %%从urdf中导入各连杆相对于对应关节坐标系的转动惯量
    param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);% I_xx
    param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(6);% I_xy
    param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);% I_xz
    param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);% I_yy
    param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(4);% I_yz
    param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);% I_zz
end
Para_vec=zeros(N_dof*10,1);
for i = 1:1:N_dof
    Para_vec(10*(i-1)+1:10*i)=param_model_matrix(:,i);

end

tau=zeros(N_dof,1000);
tau_ideal=zeros(N_dof,1000);
tau_est = zeros(N_dof,1000);
K = zeros(N_dof,N_dof*10,1000);

%手动输入辨识参数
para_estimate = [0;0;0;0;0;0;0;0;0;0.00609219526641635;1.09518027579605e-05;-0.000516603831923268;0.0276746042135597;-0.127416284239075;0.0104089324338423;-0.00144740459174840;0.00669900275173220;0.00628339966420316;0.00140894958077726;-0.000188280750154790;0.00772726264508295;0.00303027199984877;0.0278159077861742;-0.0347549440001522;0.00671964912046477;-8.93751910481942e-05;0.000152082441328492;0.00384271580536163;0.000924111846729846;0.00345792683494120];
para_estimate =[0.997646955114846;0;0;0;0.998893717171662;0;0;0.998893717171662;0;0.00249352411363238;1.58995694592197;0.0711405863799068;0.0249456751280392;-0.0496876171377602;0.00621078806632358;-0.00129731349204144;0.00242392606258014;0.00823086915717363;0.00124521383568214;0.00688848441493633;1.30320504770405;0.00303027200000040;0.0305448368719603;-0.0347549439999998;0.00625407347364987;-8.93751905266446e-05;0.000152082441216310;0.00384271580543927;0.000924111846657566;0.00299235118782368];
para_estimate = Para_vec;
AA = 120*pi/180;
ff = 2;
t_duration=10;
for i=1:1:1000*t_duration
    for j = 1:N_dof
        q(j,i) = AA*sin(2*pi*ff*i/1000);
        dq(j,i) =ff*AA*cos(2*pi*ff*i/1000);
        ddq(j,i) =-ff*ff*AA*sin(2*pi*ff*i/1000);
%         q(j,i) = pi/6*sin(2*pi*i/1000);
%         dq(j,i) =pi/3*pi*cos(2*pi*i/1000);
%         ddq(j,i) =-2*pi/3*pi*pi*sin(2*pi*i/1000);
%         q(j,i) = 0;
%         dq(j,i) =0;
%         ddq(j,i) =0;
    end
%     q(j,1) = 0.1*AA*sin(2*pi*ff*i/1000);
%     dq(j,1) =0.1*ff*AA*cos(2*pi*ff*i/1000);
%     ddq(j,1) =-0.1*ff*ff*AA*sin(2*pi*ff*i/1000);
        
    [U,Uj] = Compute_LinearDynmatrix(q(:,i), dq(:,i), ddq(:,i),robot_para);
    tau_temp= Uj *Para_vec;
    tau_est_temp = Uj * para_estimate; 
    tau_ideal_temp = inverseDynamics(robot,q(:,i),dq(:,i),ddq(:,i));
    tau(:,i)=tau_temp;
    tau_ideal(:,i)=tau_ideal_temp;
    tau_est(:,i) = tau_est_temp;
    K(:,:,i) = Uj;
    if i == 200
        c=1;
    end
end

%高斯噪声模拟数据
tau_noise = zeros(N_dof,1000*t_duration);
for i=1:1:1000*t_duration
    tau_noise(:,i) = tau_ideal(:,i) + 0.05*normrnd(0,1,N_dof,1) + sign(dq(:,i))*0.08;
    
end

figure(1)
for j=1:N_dof
    subplot(2,3,j)
    t = 0.001:0.001:t_duration;
    plot(t,tau(j,:),'-r','LineWidth',3)
    
    hold on
    plot(t,tau_ideal(j,:),'--g','LineWidth',3)
    
    hold on
    plot(t,tau_noise(j,:),'-.b')
    
    xlabel('t(s)');
    ylabel('joint torque(Nm)')
end

figure(2)
for j=1:N_dof
    subplot(2,3,j)
    t = 0.001:0.001:t_duration;
    plot(t,tau_est(j,:),'-r','LineWidth',3)
    
    hold on
    plot(t,tau_ideal(j,:),'--g','LineWidth',3)
    
    hold on
    plot(t,tau_noise(j,:),'-.b')
    
    xlabel('t(s)');
    ylabel('joint torque(Nm)')
end

%最小二乘估计动力学参数
n_data = t_duration*1000;
Kn = zeros(n_data * N_dof,N_dof*10);
tau_data = zeros(n_data*N_dof, 1);
for i=1:1:n_data
    Kn((i-1)*N_dof+1:i*N_dof,:) = K(:,:,i); %线性动力学观测矩阵
    tau_data((i-1)*N_dof+1:i*N_dof,1) = tau_noise(:,i);  %关节力矩观测向量
end

para_estimate = (Kn'*Kn + 10^-7*eye(10*N_dof))\Kn'*tau_data;
% para_estimate = inv(Kn'*Kn)*Kn'*tau_data
para_estimate_matrix = reshape(para_estimate,10,N_dof);




